/**
 * mLibQuat.js
 * Created: December 2013
 * Source link: http://code.google.com/p/code-depository/source/browse/
 *
 * By Suresh Nalluri,
 * License: The MIT License (MIT)
 *          See LICENSE details
  *
 * Requires:
 *	mLibFactory.js
 *	mLibIncludes.js
 *  mLIBVec3.js
 *  mLibMat33.js
 *
 * note this constructor pattern allows us to create objects in 2 ways:
 * 1. var o = mLib.Quat(...);      //OR
 * 2. var o = new mLib.Quat(...);
 *
 * NOTE: var o = [new] mLIB.Quat(<no params>); // will return default vector (1.0, 0.0, 0.0);
 *
 * added benefit of this pattern is that we get a copy constructor by default!!
 * eg. var newQuat = mLIB.Quat(existingQuat);
 * so no need to have an explicit copyOf method.
 *
 * Usage: 
 *		var o = [new] mLIB.Quat([ **** | [] ])
 *
 *-------------------------------------------------------
 **/
if (typeof mLIB == 'undefined') function mLIB() {};

( function(module) {
    var Quat = module.Quat,
        Vec3 = module.Vec3,
        Mat33 = module.Mat33,
        sqrt = module.Includes.sqrt,
        eqZero = module.Includes.isZero,
        cos = module.Includes.cos,
        sin = module.Includes.sin,
        acos = module.Includes.acos,
        ZERO = module.Includes.ZERO,
        axisIdx = ['x', 'y', 'z'];
    
    module.Quat = Factory.obj( {
        init: function() {
            nar nargs = arguments.length;
            
            if ( nargs == 4) {
                // 4 args given. assume we have w, x, y, z
                this.w = w;
                this.x = x;
                this.y = y;
                this.z = z;
                
            } else if ( nargs == 2 && arguments[0].isClass(Vec3) ) {
                if ( arguments[1].isClass(Vec3) ) {
                    // we have from, to vectors
                    this.setByFromToVectors(arguments[0], arguments[1]);
                    
                } else {
                    // we have axis, angle
                    this.setByAxisAngle(arguments[0], arguments[1]);
                    
                }
            } else if (nargs == 1) {
                if ( arguments[0].isClass(Vec3) ) {
                    // we have a vector
                    this.w = 0.0;
                    this.x = arguments[0].x;
                    this.y = arguments[0].y;
                    this.z = arguments[0].z;
                    
                } else {
                    // assume its a Mat33 rotation matrix
                    this.setByMatrix(arguments[0]);
                }
            } else {
                // either no args given or we have some unknown params
                // either case, return identity
                this.w = 1.0;
                this.x = this.y = this.z = 0.0;
            }
            
        }, // init()

        identity: function() {
            this.w = 1.0;
            this.x = this.y = this.z = 0.0;
            
            return this;
        },
        
        zero: function() {
            this.w = this.x = this.y = this.z = 0.0;
            return this;
        },
        
        copyQuat: function(q) {
            this.w = q.w;
            this.x = q.x;
            this.y = q.y;
            this.z = q.z;
            
            return this;
        },
        
        // set methods
        set: function(w, x, y,z) {
            this.w = w;
            this.x = x;
            this.y = y;
            this.z = z;
            
            return this;
        },
        
        setByvector: function(v) {
            this.w = 0.0;
            this.x = v.x;
            this.y = v.y;
            this.z = v.z;
            
            return this;
        },
        
        setByMatrix: function(rot) {
            // assumptions: R is a 3x3 rotation matrix
            var t = rot.trace(),
                R = Mat33.$reader();
                
            if ( t > 0 ) {
                var s = sqrt(1.0 + t),
                    r = 0.5 / s;
                
                this.w = 0.5*s;
                this.x = ( R(2,1)-R(1,2) )*r;   // ( R[5]-R[7] )*r; (  - Note: R(i,j) = R[i+3*j]
                this.y = ( R(0,2)-R(2,0) )*r;   // ( R[6]-R[2] )*r;
                this.z = ( R(1,0)-R(0,1) )*r;   // ( R[1]-R[3] )*r;
                
            } else {
                var i = 0;
                if ( R(1,1) > R(0,0) ) i = 1;
                if ( R(2,2) > R(i,i) ) i = 2;
                var j = (i+1)%3,
                    k = (j+1)%3,
                    s = sqrt( R(i,i) - R(j,j) - R(k,k) + 1.0 ),
                    r = 0.5 / s;
                    
                w = ( R(k,j)-R(j,k) )*r;
                this[axisIdx[i]] = 0.5*s;
                this[axisIdx[j]] = ( R(j,i) + R(i,j) )*r;
                this[axisIdx[k]] = ( R(k,i) + R(i,k) )*r;
            }
            
            return this;

        },
        
        setByFixedAngles: function(zRot, yRot, xRot) {
            // set half angles
            zRot *= 0.5;
            yRot *= 0.5;
            xRot *= 0.5;

            // get sines and cosines of half angles
            var cx = cos(xRot), sx = sin(xRot),
                cy = cos(yRot), sy = sin(yRot),
                cz = cos(zRot), sz = sin(zRot);

            this.w = cx*cy*cz - sx*sy*sz;
            this.x = sx*cy*cz + cx*sy*sz;
            this.y = cx*sy*cz - sx*cy*sz;
            this.z = cx*cy*sz + sx*sy*cx;
            
            return this;
            
        },
        
        // set quaternion based on start and end vectors (both of type Vec3)
        // Algorithm based on notes on gamedev.net and euclideanspace.com, by minorlogic.
        // Note: no unit vectors required
        //
        setByFromToVectors: function(fromVec, toVec) {
            // get axis of rotation
            var axis = fromVec.Cross( toVec );

            // initialise our quaternion - note w is the scaled COS(angle between vectors)
            this.set(fromVec.dot( toVec), axis.x, axis.y, axis.z);
            
            // we now have the quat : |fromVec|*|toVec|*( cos(angle), r*sin(angle) )
            // so let's normalise
            this.normalise();   // to get ( cos(angle), r*sin(angle) )
            
            // but remember we need to use half-angles.. ie we need ( cos(angle/2), r*sin(angle/2) )
            
            // lets do that now:
            this.w += 1.0;
            
            // if we normalise now we'll divide by sqrt(2*(1+cos(angle))),
            // which is what we're aiming for for r*sin(angle) to give
            // r*sin(angle/2).
            
            // and w = (1+cos(angle)) / sqrt(2*(1+cos(angle))) = cos(angle/2)  !!
            
            //lets check if vectors are opposing each other first
            if ( eqZero(this.w) {
                // rotate pi around the orthogonal vector
                
                // either cross prod with x axis
                if ( fromVec.z*fromVec.z > fromVec.x*fromVec.x )
                    this.set(0.0, 0.0, fromVec.z, -fromVec.y);
                else
                    // or cross prod with z-axis
                    this.set(0.0, fromVec.y, -fromVec.x, 0.0);
            }
            
            return this.normalise();
            
        },

        // set quaternion based on Axis and angle
        setByAxisAngle: function(axis, theta) {
        
            var lenSq = axis.lengthSquared();
            // if axis of roation is a zero vector then set quat  to identity
            if ( eqZero( lenSq ) ) {
                this.w = 1.0;
                this.x = this.y = this.z = 0.0;
                return this;
            }
            
            // set the quat .. rem to take the half angle
            var f = sin(0.5*theta) / sqrt( lenSq );
            
            this.w = cos(halfTheta);
            this.x = f * axis.x;
            this.y = f * axis.y;
            this.z = f * axis.z;
            
            return this;
        },
        
        // return axis-angle for this quat
        getAxisAngle(axis) {    // axis vector return in the axis object param
            var angle = 2.0*acos( this.w ),
                length = sqrt( 1.0 - this.w*this.w );
            
            if ( eqZero( length ) ) {
                axis.x = axis.y = axis.z = 0.0; // axis.zero()
            } else {
                length = 1.0 / length;
                axis.x = this.x * length;
                axis.y = this.y * length;
                axis.z = this.z * length;
            }
            
            return angle;
        }
        
        norm: function() {
            return this.w*this.w + this.x*this.x + this.y*this.y + this.z*this.z;
        },
        
        magnitude: function() {
            return sqrt( this.w*this.w + this.x*this.x + this.y*this.y + this.z*this.z );
        },
        
        isEqual(q) {
            return (
				eqZero(this.x - q.x) &&
				eqZero(this.y - q.y) &&
				eqZero(this.z - q.z) &&
				eqZero(this.w - q.w)
            );
            
        },

        isNotEqual: function(v) {
            return !(
				eqZero(this.x - q.x) &&
				eqZero(this.y - q.y) &&
				eqZero(this.z - q.z) &&
				eqZero(this.w - q.w)
            );
        },

        // check is quat is the zero quat
        isZero: function() {
            return eqZero( this.w*this.w+this.x*this.x+this.y*this.y+this.z*this.z );
            
        },
        
        isUnit: function() {
            return eqZero( this.w*this.w-this.x*this.x-this.y*this.y-this.z*this.z );
        },
        
        isIdentity: function() {
            return (
                eqZero(1.0 - this.w) &&
                eqZero(x) &&
                eqZero(y) &&
                eqZero(z) &&
            );
        },
        
        // clean the quat - ie. set elements to zero that are close to zero
        roundToZero: function() {
            eqZero( this.x ) && (this.x = 0.0);
            eqZero( this.y ) && (this.y = 0.0);
            eqZero( this.z ) && (this.z = 0.0);
            eqZero( this.w ) && (this.w = 0.0);
            return this;

        },
        
        normalise: function() {
            var lenSq = this.w*this.w + this.x*this.x + this.y*this.y + this.z*this.z;

            // if lenSq is zero, then zero *this* and return
            if ( eqZero( lenSq ) )
            {
                this.w = this.x = this.y = this.z = 0.0;
                return this;
            }

            var f = 1.0 / sqrt( lenSq );
            
            this.w *= f;
            this.x *= f;
            this.y *= f;
            this.z *= f;
            
            return this;

        },
        
        Normalise: function() {
            var lenSq = this.w*this.w + this.x*this.x + this.y*this.y + this.z*this.z;

            // if lenSq is zero, then return a zero quat
            if ( eqZero( lenSq ) ) return new Quat(0.0, 0.0, 0.0, 0.0);
            
            var f = 1.0 / sqrt( lenSq );
            
            return new Quat( this.w*f, this.x*f, this.y*f, this.z*f );
            
        },
        

        conjugate: function() {
            this.x = -this.x;
            this.y = -this.y;
            this.z = -this.z;

            return this;
        },
        
        Conjugate: function() {
            return new Quat(
                this.w,
                -this.x,
                -this.y,
                -this.z
            );
        
        },
        
        inverse: function() {
            var w = this.w,
                x = this.x,
                y = this.y,
                z = this.z,
                recip = 1.0 / (w*w + x*x + y*y + z*z);  //we assume *this* is not a zero quat

            this.w *= recip;
            this.x *= -recip;
            this.y *= -recip;
            this.z *= -recip;
            
            return this;
            
        },
        
        Inverse: function() {
            var w = this.w,
                x = this.x,
                y = this.y,
                z = this.z,
                recip = 1.0 / (w*w + x*x + y*y + z*z);  //we assume *this* is not a zero quat
            
            return new Quat( recip*w, -recip*x, -recip*y, -recip*z );
        },
        
        plus: function() {
            this.w += q.w,
            this.x += q.x,
            this.y += q.y,
            this.z += q.z
            
            return this;
        },
        
        Plus: function() {
           return new Quat(
                this.w + q.w,
                this.x + q.x,
                this.y + q.y,
                this.z + q.z
            );

        },
        
        minus: function(q) {
            this.w -= q.w,
            this.x -= q.x,
            this.y -= q.y,
            this.z -= q.z
            
            return this;
            
        },
        
        Minus: function(q) {
            return new Quat(
                this.w - q.w,
                this.x - q.x,
                this.y - q.y,
                this.z - q.z
            );
            
        },
        
        negate: function() {
            this.w = -this.w;
            this.x = -this.x;
            this.y = -this.y;
            this.z = -this.z;
            
            return this;
        },
        
        Negate: function() {
            return new Quat( -this.w, -this.x, -this.y, -this.z );
            
        },
        
        //multiply by scalar and set self to product
        scale: function(sV) {
        
            this.w *= sV;
            this.x *= sV;
            this.y *= sV;
            this.z *= sV;
            
            return this;
        },
        
        // multiply by scalar and return new quat
        Scale: function(sV) {
            return new Quat( sV*this.w, sV*this.x, sV*this.y, sV*this.z );
        
        },
        
        // multiply with a quaternion and set self to product
        multiply: function(q) {
            var w = this.w,
                x = this.x,
                y = this.y,
                z = this.z;
            
            this.w = w*q.w - x*q.x - y*q.y - z*q.z;
            this.x = w*q.x + x*q.w + y*q.z - z*q.y;
            this.y = w*q.y + y*q.w + z*q.x - x*q.z;
            this.z = w*q.z + z*q.w + x*q.y - y*q.x;
            
            return this;

        },
        
        // multiply *this* with a quaternion and retrun new
        Multiply: function(q) {
            var w = this.w,
                x = this.x,
                y = this.y,
                z = this.z;
                
            return new Quat(
                w*q.w - x*q.x - y*q.y - z*q.z,
                w*q.x + x*q.w + y*q.z - z*q.y,
                w*q.y + y*q.w + z*q.x - x*q.z,
                w*q.z + z*q.w + x*q.y - y*q.x
            );
        },
        
        dot: function(q) {
            return ( this.w*q.w + this.x*q.x + this.y*q.y + this.z*q.z);
        },
        
        // return the vector rotated by *this* quaternion
        Rotate: function(vec) {
            // assumptions: *this* is a normalised
            var v = 2.0*(this.x*vec.x + this.y*vec.y + this.z*vec.z),
                c = 2.0*w,
                p = c*this.w - 1.0;

            return new Vec3(
                p*vec.x + v*this.x + c*(this.y*vec.z - this.z*vec.y),
                p*vec.y + v*this.y + c*(this.z*vec.x - this.x*vec.z),
                p*vec.z + v*this.z + c*(this.x*vec.y - this.y*vec.x)
            );

        },
                
		toArray: function() {
			return [this.w, this.x, this.y, this.z].join('');
		},

        log: function() {
			return ['(', this.w, ',', this.x, ',', this.y, ',', this.z, ')'].join("");
        },

	} );  //mLIB.Quat = Factory.obj(

    // friend methods ---------------------------------------------
    //
    var friends = {
        dot: function(q1, q2) {
            return (q1.w*q2.w + q1.x*q2.x + q1.y*q2.y + q1.z*q2.z);
        },
        
        // multiply given quat by a scalar and return new quat
        Scale: function(sV, q) {
            return new Quat( sV*q.w, sV*q.x, sV*q.y, sV*q.z );
        },
        
        scale: function(sv, q) {
            q.w *= sV;
            q.x *= sV;
            q.y *= sV;
            q.z *= sV;
            
            return q;
        }
        Inverse: function(q) {
            var norm = q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z;
            
            // if q is a zero quaternion, return identity
            if ( eqZero( norm ) )
            {
                return new Quat();
            }

            var recip = 1.0 / norm;
            return new Quat( recip*q.w, -recip*q.x, -recip*q.y, -recip*q.z );
        },
        
        Conjugate: function(q) {
            return new Quat(q.w, -q.x, -q.y, -q.z);
            
        },
        
        Normalise: function(q) {
            var lenSq = q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z;

            // if lenSq is zero, then return a zero quat
            if ( eqZero( lenSq ) ) return new Quat(0.0, 0.0, 0.0, 0.0);
            
            var f = 1.0 / sqrt( lenSq );
            
            return new Quat( q.w*f, q.x*f, q.y*f, q.z*f );
            
        },
        
        Multiply: function(q1, q2) {
        },
        
        // interpolation methods
        
        // linearly interpolate 2 quats
        lerp: function(qStart, qEnd, t) {
        
            var cosTheta = qStart.dot( qEnd ), // cos of angle between quaternions
                quat = qEnd.Scale(t);

            // if angle between quaternions is less than 90 degrees
            if ( eqZero(cosTheta) ) {
                // use standard interpolation
                quat.add(qStart.Scale(1.0-t));
            } else {
                // otherwise, take the shorter path
                quat.add(qStart.Scale(t-1.0));
            }
            
            return quat;
            
        },
        
        // spherical linear interpolation
        slerp: function(qStart, qEnd, t) {
            var cosTheta = qStart.dot( qEnd ), // cos of angle between quaternions
                iStart, iEnd,   // start interpolation and end interpolation
                quat;
                
            // if angle between quaternions is less than 90 degrees
            if ( eqZero(cosTheta) ) {
                // if angle is greater than zero
                if ( (1.0 - cosTheta) > ZERO )
                {
                    // use standard slerp
                    var theta = acos( cosTheta ),
                        rSinTheta = 1.0 / sin( theta );

                    iStart = sin( (1.0 - t)*theta )*rSinTheta;
                    iEnd   = sin( t*theta )*rSinTheta;
                }
                // angle is close to zero
                else
                {
                    // use linear interpolation
                    iStart = 1.0 - t;
                    iEnd   = t;
                }
            } else {
                // take the shorter route
                
                // if angle < 180 degrees
                if ( (1.0 + cosTheta) > ZERO )
                {
                    // use slerp w/negation of start quaternion
                    var theta = acosf( -cosTheta ),
                        rSinTheta = 1.0 / sin( theta );

                    iStart = sin( (t-1.0)*theta )*rSinTheta;
                    iEnd   = sin( t*theta )*rSinTheta;
                }
                // angle is close to 180 degrees
                else
                {
                    // use lerp w negation of start quaternion
                    iStart = t - 1.0;
                    iEnd = t;
                }
            }
            
            return Quat(qStart).scale(iStart).plus(qEnd.Scale(iEnd));
            
        },
        
        // approximate slerp; see http://number-none.com/product/Hacking%20Quaternions/
        approxSlerp: function(qStart, qEnd, t) {
        
            var cosTheta = qStart.dot( qEnd ), // cos of angle between quaternions
        
                f = 1.0 - 0.7878088f*cosTheta,  // correct time by using cosine of angle between quaternions
                k = 0.5069269 * f*f,
                
                b = 2*k,
                c = -3*k,
                d = 1 + k,

                t = t*(b*t + c) + d,

                quat = qEnd.Scale(t);   // initialize

            // if "angle" between quaternions is less than 90 degrees
            if ( eqZero(cosTheta) ) {
                // use standard interpolation
                quat.add(qStart.Scale(1.0-t));
            } else {
                // otherwise, take the shorter path
                quat.add(qStart.Scale(t-1.0));
            }
            
            return quat;

        },
                
        // some static stuff
        ZERO: new Quat(0.0, 0.0, 0.0, 0.0),
        IDENTITY: new Quat(1.0, 0.0, 0.0, 0.0)
        
    }; // friends{}

    for (var f in friends) {
        friends.hasOwnProperty(f) && (module.Vec4[f] = friends[f]);
    };

} )(mLIB);